#include "contiobsmap.h"

Gridmap contiObsMap_dymap_G(const matric &obs,const int &dx, const int &dy, const int &rsafe, const vec &zmin, const vec &zmax,const vector<int> &nz, vector<float> &Ahmap, vector<vector<float>>pilerange)
{
    int n1=nz[0];
    int n2=nz[1];
    vector<float> gMap(n1*n2,rsafe+4);
    double deltax=(zmax[0]-zmin[0])/n1;
    double deltay=(zmax[1]-zmin[0])/n2;

    //遍历每个障碍物，计算各个栅格距离障碍物的最近距离

    for(const auto& obsk:obs)
    {
        float xK=obsk[0];
        float yk=obsk[1];
        double minIX=(xK-dx-zmin[0])/deltax +0.5;
        double maxIX=(xK+dx-zmin[0])/deltax +0.5;
        double minIY=(yk-dy-zmin[1])/deltay +0.5;
        double maxIY=(yk+dy-zmin[1])/deltay +0.5;
        vector<int> IX_,IY_;
        for(int i=floor(minIX);i<ceil(maxIX);i++)
        {
            IX_.push_back(i);
        }
        for(int i=floor(minIY);i<ceil(maxIY);i++)
        {
            IY_.push_back(i);
        }
        float ddx=zmin[0]-obsk[0];
        float ddy=zmin[1]-obsk[1];
        for(int i=0;i<IX_.size();i++)
        {
            for(int j=0;j<IY_.size();j++)
            {
                float dd1=ddx+IX_[i]*deltax;
                float dd2=ddy+IY_[j]*deltay;
                float dis=sqrt(dd1*dd1+dd2*dd2);
                int ixy=IX_[i]+nz[0]*IY_[j];
                if(dis<gMap[ixy])
                {
                    gMap[ixy]=dis;
                }
            }
        }

    }
    float drmax=0.5;
    vector<float> pilemin=pilerange[0];
    vector<float> pilemax=pilerange[1];
    matric zz;
    vector<int> idzmap;
    fsg3d1(nz,zmin,zmax,pilemin,pilemax,zz,idzmap);
    vector<vector<float>> dis2obs =mindisobs(zz,gMap,nz,zmin,zmax);
    vector<int> hmap;
    vector<int> flagkxcar;
    for(int i=0;i<dis2obs.size();i++)
    {
        float hmap_temp1=10*max(float(0),rsafe-dis2obs[i][0])/rsafe+2*max(float(0),rsafe+drmax-dis2obs[i][0])/drmax+\
        1*max(float(0),rsafe+2*drmax-dis2obs[i][0])/drmax+0.25*max(float(0),rsafe+3*drmax-dis2obs[i][0])/drmax+\
        0.125*max(float(0),rsafe+4*drmax-dis2obs[i][0])/drmax;

        float hmap_temp2=10*max(float(0),rsafe-dis2obs[i][1])/rsafe+2*max(float(0),rsafe+drmax-dis2obs[i][1])/drmax+\
        1*max(float(0),rsafe+2*drmax-dis2obs[i][1])/drmax+0.25*max(float(0),rsafe+3*drmax-dis2obs[i][1])/drmax+\
        0.125*max(float(0),rsafe+4*drmax-dis2obs[i][1])/drmax;
        int hmap_temp=ceil(max(hmap_temp1/Ahmap[0]*8,hmap_temp2/Ahmap[1]*8));
        hmap.push_back(hmap_temp);
        if(dis2obs[i][0>rsafe&&dis2obs[i][1]>rsafe])
        {
            flagkxcar.push_back(1);
        }
        else
        {
            flagkxcar.push_back(0);
        }

    }
    Gridmap result;
    result.hmap=hmap;
    result.flagkxcar=flagkxcar;
    return result;
    

}

/*int main()
{
    matric obs1={{11,22},{11,13},{11,11},{12,12}};
    int dx=3,dy=3;
    int rsafe=1.71;
    vector<float> zmin={-10,-20};
    vector<float> zmax={10,20};
    vector<int> nz={400,400,50};
    vector<float> Ahmap={10,20};
    vector<vector<float>> pilerange={{-5,-10},{5,10}};  
    Gridmap result=contiObsMap_dymap_G(obs1, dx, dy, rsafe, zmin, zmax,nz, Ahmap, pilerange);



    return 0;
}*/